#include "ctrlMapHostFactoryTest.h"
#include "module_config.h"
#include "wl_version.h"

#pragma pack(1)
typedef struct{
    char bat_Pn[20];         //battry bms PN , reserve for 20byte
    uint16_t bat_rated_cap;
    uint16_t total_vol;
    uint8_t temp_sensor_num;
    int16_t cell_temp1;
    int16_t cell_temp2;
    int16_t cell_temp3;
    uint8_t serial_num;
    uint16_t cell_vol[10];
    uint16_t soh_user;
    uint16_t cycle;
    uint32_t bat_activate_time;
    int16_t current;
}Bat_Infor_Data_t;
#pragma pack()

uint16_t g_u16FacLiftMotorCtrl = 0;
uint16_t g_u16FacLiftMotorResult = 0;

static void switchFactoryMode(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    uint16_t u16FacModeCtl = *(uint16_t*)pu8Data;

    if (/* gmowerData.powerOffState==1 || */get_ota_status() == 1 || get_factory_status(FACTORY_TYPE_FACCHECK) == 1)
    {
        return;
    }

    if (u16FacModeCtl == 1)
    {
        set_factory_status(FACTORY_TYPE_BOARDCHECK, 1);
    }
    else if (u16FacModeCtl == 0)
    {
        set_factory_status(FACTORY_TYPE_BOARDCHECK, 0);
    }
}

#if defined (MODULE_INCLUDE_LED)
static void colorEffectTest(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    g_u16FacLightTestFlag = *(uint16_t*)pu8Data;
}
#endif

#if defined (MODULE_INCLUDE_SCREEN)
static void screenDisplayTest(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    g_u16FacScreenTestFlag = *(uint16_t *)pu8Data;
}
#endif

#if defined (MODULE_INCLUDE_KEY)
static void stopKeyCheck(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        uint16_t trigger = g_emergency_info.key_state;
        packData2Ap((uint8_t *)&trigger, 2, COMM_PDA_STOP_KEY_CHECK, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void funKeyCheck(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    const uint16_t KEY2ID[KEY_MAX + 1] = {2, 3, 1, 0};
    uint16_t u16Tmp;
    int i;

    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        for (i = 0; i < KEY_MAX; i++)
        {
            if (KEY_STA_PRESSED == astKey[i].u8StableState)
            {
                break;
            }
        }

        u16Tmp = KEY2ID[i];     
        packData2Ap((uint8_t *)&u16Tmp, 2, COMM_PDA_KEY_CHECK, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}
#endif

#if defined (MODULE_INCLUDE_SUBG)
static void chargeStationPn(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
   int8_t ret;

   if ((COMM_CMD_CAMP_WR == u8Cmd) || (COMM_CMD_CAMP_WR_NR == u8Cmd))
   {
       if (u8DataSize != PN_LEN)
       {
           LOG(AM_LOG_LEVEL_INFO, "FAC", "set stationpn len err %d\r\n", u8DataSize);
       }
       
       ret = config_data_init();
       memcpy(gDataConfig.pairinfo.slave_pn[0], pu8Data, PN_LEN);
       ret = write_cfg_data(&gDataConfig);
       LOG(AM_LOG_LEVEL_INFO, "FAC", "set stationpn ret %d, pn: ", ret);
       LOG_STR(gDataConfig.pairinfo.slave_pn[0], PN_LEN); 
   }
   else if (COMM_CMD_CMAP_RD == u8Cmd)
   {
       config_data_init();
       packData2Ap(gDataConfig.pairinfo.slave_pn[0], PN_LEN, COMM_PDA_CHARGE_STATION_PN, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
   }
}

static void readSubgRXSignal(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    uint16_t u16Tmp = getSiRSSI();
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)&u16Tmp, 2, COMM_PDA_SUB_SIGNAL_RX_STRENGTH, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void readSubgTXSignal(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    uint16_t u16Tmp = DeviceRxRSSI[0].sta_rssi;
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)&u16Tmp, 2, COMM_PDA_SUB_SIGNAL_TX_STRENGTH, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void subgCfg(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    uint16_t  u16Cfg = 0;
    if ((COMM_CMD_CAMP_WR == u8Cmd) || (COMM_CMD_CAMP_WR_NR == u8Cmd))
    {
        config_data_init();
        u16Cfg = *(uint16_t*)pu8Data;
        if (915 == u16Cfg)
        {
            // memcpy(&gDataConfig.old_subgCfg,"s915",COMM_PDA_ECU_SUBG_CFG_LEN);
            gDataConfig.subg_band = SUBG_BAND_915;
        }
        else if (868 == u16Cfg)
        {
            // memcpy(&gDataConfig.old_subgCfg,"s868",COMM_PDA_ECU_SUBG_CFG_LEN);
            gDataConfig.subg_band = SUBG_BAND_868;
        }
        else
        {
            LOG(AM_LOG_LEVEL_INFO, "FAC", "set subg cfg err %d\r\n", u16Cfg);
            return;
        }

        int8_t ret_cfg = write_cfg_data(&gDataConfig);
        if(ret_cfg)
        {
            LOG_TS(AM_LOG_LEVEL_FATAL, "FAC","subg cfg write config_data failed,%d!\n\r",ret_cfg);
        }
        ret_cfg = config_data_init();
        if(ret_cfg)
        {
            LOG_TS(AM_LOG_LEVEL_FATAL, "FAC","subg cfg read config_data failed,%d!\n\r",ret_cfg);
        }
    }
    else if (COMM_CMD_CMAP_RD == u8Cmd)
    {     
        config_data_init();
        u16Cfg = gDataConfig.subg_band;
        packData2Ap((uint8_t *)&u16Cfg, 2, COMM_PDA_ECU_SUBG_CFG, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void readSubgRxRSSI(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    int16_t ret =0;

    uint8_t bsu_id =0;
    bsu_id = *(uint8_t *)pu8Data;
    
    for(uint8_t i =0;i<gDataConfig.pairinfo.paired_num;i++)
    {
        if(PairDevice[i].DeviceID == bsu_id)
        {
            if(get_device_online_status(PairDevice[i].DeviceID) == DEVICE_ONLINE)
            {
                ret = DeviceRxRSSI[i].ecu_rssi;
            }
            else
            {
                ret = 0;
            }
            break;
        }
    }

    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)&ret, 2, COMM_SALED_SUBG_RX_RSSI, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void readSubgTxRSSI(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    int16_t ret =0;

    uint8_t bsu_id =0;
    bsu_id = *(uint8_t *)pu8Data;
    
    for(uint8_t i =0;i<gDataConfig.pairinfo.paired_num;i++)
    {
        if(PairDevice[i].DeviceID == bsu_id)
        {
            if(get_device_online_status(PairDevice[i].DeviceID) == DEVICE_ONLINE)
            {
                ret = DeviceRxRSSI[i].sta_rssi;
            }
            else
            {
                ret = 0;
            }
            break;
        }
    }

    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)&ret, 2, COMM_SALED_SUBG_TX_RSSI, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void rcvPdaPaircmd(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    uint8_t pair_cmd =0;
    uint8_t cmd_id =0;
    uint8_t ctrl =0;
    if ((COMM_CMD_CAMP_WR == u8Cmd) || (COMM_CMD_CAMP_WR_NR == u8Cmd))
    {
        pair_cmd = pu8Data[0];
        if(pair_cmd == 0x01)
        {
            LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Paircmd #######enter %d\r\n", pair_cmd);
            NormalBroadcast();
            setSubGIntoPairStatus();
        }
        else if(pair_cmd == 0x02)
        {
            LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Paircmd #######exit %d\r\n", pair_cmd);
            setSubGExitPairStatus();
        }
        else if(pair_cmd == 0x03)
        {
            cmd_id = pu8Data[1];
            LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Paircmd #######remove %d %d\r\n", pair_cmd,cmd_id);
            removeSubGPairInfo(cmd_id);
        }
        else if(pair_cmd == 0x04)
        {
            cmd_id = pu8Data[1];
            ctrl = pu8Data[2]; 
            LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Paircmd ######flash %d %d\r\n", pair_cmd,cmd_id);
            flashLedToPairedDevice(cmd_id,ctrl);
        }
        else if(pair_cmd == 0x08)
        {
            LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Paircmd #######factory pair %d %s\r\n", pair_cmd,&pu8Data[1]);
            getUpperCpuSN();
            TargetedBroadcastEnable(&pu8Data[1],16);
            setSubGIntoPairStatus();
        }
        else if(pair_cmd == 0x09)
        {
            LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Paircmd #######remove All PairInfo\r\n");
            removeAllSubGPairInfo();
        }
        else if(pair_cmd == 0x0A)
        {
            LOG_TS(AM_LOG_LEVEL_FATAL,"Paircmd","#######Set PairInfo\r\n");
            uint16_t len = *(uint16_t *)(pu8Data+1);
            uint8_t num = *(pu8Data+3);
            uint8_t size = *(pu8Data+4);
            
            apSetSubGPairInfo(len,num,size,(pu8Data+5));
        }
    }
}
#endif

static void pdaShutDown(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if ((COMM_CMD_CAMP_WR == u8Cmd) || (COMM_CMD_CAMP_WR_NR == u8Cmd))
    {
        if (1 == *(uint16_t*)pu8Data)
        {
            LOG_TS(AM_LOG_LEVEL_WARNING, "FAC", "shut down! as OK LONG\r\n");
            // g_power_down_flag = 2;
        }
    }
}

static void getEcuVersion(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    uint16_t u16Ver = WL_SW_VERSION;
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        /* ECU Version */
        packData2Ap((uint8_t *)&u16Ver, 2, COMM_PDA_ECU_FIRMWARE_VERSION, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void getEcuPn(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        /* ECU PN */
        packData2Ap((uint8_t *)g_factory_nessary.pnArray, PN_LEN, COMM_PDA_ECU_PN, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void getEcuRandomNum(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        /* ECU random num */   
        packData2Ap((uint8_t *)g_factory_nessary.randomNumArray, 6, COMM_PDA_ECU_RANDOM_NUM, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void getCpuId(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        /* ECU cpuid */
        BspCpuId_st *cpuid = bsp_cpu_id_read();
        packData2Ap((uint8_t *)cpuid, 12, COMM_PDA_ECU_CPUID, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

#if defined (MODULE_INCLUDE_BMS)
static void getBatSoc(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)&g_u16BatSOC, 2, COMM_PDA_BAT_SOC, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void getBatErr(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)&g_u16BatErrorCode, 2, COMM_PDA_BAT_ERROR, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void getBatParallel(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        uint16_t tmpParallel = Cell_Infor_Data.cell_parallel;
        packData2Ap((uint8_t *)&tmpParallel, 2, COMM_PDA_BAT_PARALLEL, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void getBatNTC(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    s16 temper = 0;
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        temper = (s16)bmsGetData()->bat_data.TS1_Temperature;
        packData2Ap((uint8_t *)&temper, 2, COMM_PDA_BAT_NTC, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void getBatMrfs(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        uint16_t tmpType = Cell_Infor_Data.cell_type;
        packData2Ap((uint8_t *)&tmpType, 2, COMM_PDA_BAT_MRFS, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void getBatPN(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)BMS_para_data.bat_Pn, BMS_PN_LEN, COMM_PDA_BAT_PN, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void getBatInfor(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    Bat_Infor_Data_t batInforData;
    memset(&batInforData, 0, sizeof(Bat_Infor_Data_t));
    memcpy(batInforData.bat_Pn, BMS_para_data.bat_Pn, BMS_PN_LEN);
    batInforData.bat_rated_cap =    BMS_para_data.bat_rated_cap;
    batInforData.total_vol = bmsGetData()->bat_data.Stack_Voltage;
    batInforData.temp_sensor_num = getCellTempProbeNum();
    if(batInforData.temp_sensor_num == 2)
    {
        batInforData.cell_temp1 = (bmsGetData()->bat_data.TS1_Temperature)*10;
        batInforData.cell_temp2 = (bmsGetData()->bat_data.TS3_Temperature)*10;
        batInforData.cell_temp3 = -273;
    }
    else if(batInforData.temp_sensor_num == 1)
    {
        batInforData.cell_temp1 = (bmsGetData()->bat_data.TS1_Temperature)*10;
        batInforData.cell_temp2 = -273;
        batInforData.cell_temp3 = -273;
    }
    batInforData.serial_num = BAT_SERIES_NUM;
    memcpy(batInforData.cell_vol, BMS_Data.bat_data.Cell_Voltage, 2* batInforData.serial_num);
    batInforData.soh_user = BMS_health_Data.bat_soh_user;
    batInforData.cycle = BMS_health_Data.bat_cycle_count;
    batInforData.bat_activate_time = 0xFFFFFFFF;
    batInforData.current = bmsGetData()->bat_data.Pack_Current;
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)&batInforData, sizeof(Bat_Infor_Data_t), COMM_PDA_BAT_INFOR, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}
#endif

#if defined (MODULE_INCLUDE_BUMPER)
static void getBumperStatus(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)&bumper.value, 2, COMM_PDA_GET_BUMPER_STATUS, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}
#endif

#if defined (MODULE_INCLUDE_LIFTUP)
static void getHallValue(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    uint16_t temp_value = 0;
    if(COMM_CMD_CMAP_RD == u8Cmd)
    {
        if((get_liftup_value() & 0x03) != 0)          //imu does not detect lift
        {
            temp_value = 0;
        }
        else
        {
            temp_value = get_liftUp_state();
            if(bmsGetChargeState() == 1)            //At the charging pile
            {
                if(temp_value == 1)
                {
                    temp_value = 0;                 //error
                }
                else
                {
                    temp_value = 1;                 //pass
                }
            }
            else
            {
                temp_value = 0;
            }
        }
        LOG_TS(AM_LOG_LEVEL_FATAL, "RMA", "Get hall value %d\n", temp_value);
        packData2Ap((uint8_t *)&temp_value, 2, COMM_SALED_RIGHT_LIFT_HALL, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}
#endif

#ifdef TCU_MODULE
static void ctrlTrimmerMotor(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if ((COMM_CMD_CAMP_WR == u8Cmd) || (COMM_CMD_CAMP_WR_NR == u8Cmd))
    {
        factoryCheck.tcu.enable = *(uint16_t*)pu8Data;
    }
}

static void getTrimmerMotorSpeed(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        const TcuFeedback_st* const feedback = tcu_get_feedback();
        packData2Ap((uint8_t *)&feedback->speed, 2, COMM_PDA_TCU_MOTOR_SPEED_FEEDBACK, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}
static void tcuMotorInsertStatus(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    uint16_t status =0;
    
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        status = PAYLOAD_DETECT_INSERT_READ();
        packData2Ap((uint8_t *)&status, 2, COMM_PDA_TCU_MOTOR_INSERT_STATUS, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}
#endif

static void getBootVersion(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    uint32_t u32Ver = getbootloaderversion();
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        packData2Ap((uint8_t *)&u32Ver, 2, COMM_PDA_ECU_BOOT_VERSION, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

#if defined (MODULE_INCLUDE_MOTOR)
static void ctrlLiftMotor(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if ((COMM_CMD_CAMP_WR == u8Cmd) || (COMM_CMD_CAMP_WR_NR == u8Cmd))
    {
        g_u16FacLiftMotorCtrl = *(uint16_t*)pu8Data;
//        if(g_u16FacLiftMotorCtrl == 1)
            g_u16FacLiftMotorResult =11;
            
    }
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        LOG_TS(AM_LOG_LEVEL_INFO, "FACTCK","RE %d!\n\r",g_u16FacLiftMotorResult);
        packData2Ap((uint8_t *)&g_u16FacLiftMotorResult, 2, COMM_PDA_LIFT_MOTOR_HALL_TEST, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void ctrlLawnMotor(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if ((COMM_CMD_CAMP_WR == u8Cmd) || (COMM_CMD_CAMP_WR_NR == u8Cmd))
    {
        g_u16FacLawnMotorCtrl = *(uint16_t*)pu8Data;
        if (g_u16FacLawnMotorCtrl)
            mowerMotorControlDataForTest.cmdType = MOTOR_CONTROL_BY_SPEED_WITH_MODEL;
        else 
            mowerMotorControlDataForTest.cmdType = MOTOR_CONTROL_BY_MODEL;
    }
    LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Rev lawn motor run ctrl:%d\r\n", g_u16FacLawnMotorCtrl);
}

static void getLawnMotorSpeed(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    int16_t value[2] = {0};
    if (COMM_CMD_CMAP_RD == u8Cmd)
    {
        fct_check_lawnmotor_state(value, sizeof(value)/sizeof(int16_t));
        packData2Ap((uint8_t *)&value[1], 2, COMM_PDA_LAWN_MOTOR_SPEED_FEEDBACK, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
}

static void ctrlRunMotor(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if ((COMM_CMD_CAMP_WR == u8Cmd) || (COMM_CMD_CAMP_WR_NR == u8Cmd))
    {
        uint8_t ctrl = *pu8Data;
        uint8_t pos = *(pu8Data + 1);

        if(ctrl == 1)
        {
            if(pos == 1)                //control front motor
            {
                mowerMotorControlDataForTest.cmdByRPM.motorRPM[0] = 540;
                mowerMotorControlDataForTest.cmdByRPM.motorRPM[1] = -540;
                mowerMotorControlDataForTest.cmdByRPM.motorRPM[2] = 0;
            }
            else if(pos == 2)          //control rear motor
            {
                mowerMotorControlDataForTest.cmdByRPM.motorRPM[0] = 0;
                mowerMotorControlDataForTest.cmdByRPM.motorRPM[1] = 0;
                mowerMotorControlDataForTest.cmdByRPM.motorRPM[2] = 1100;
            }

            mowerMotorControlDataForTest.cmdByRPM.keepAliveEnable = 1;
            mowerMotorControlDataForTest.cmdType = MOTOR_CONTROL_BY_RPM;
        }
        else if(ctrl == 2)
        {
            for(uint8_t i = 0;i < 3;i++)
            {
                mowerMotorControlDataForTest.cmdByRPM.motorRPM[i] = 0;
            }
            mowerMotorControlDataForTest.cmdByModelSpeed.v = 0;
            mowerMotorControlDataForTest.cmdByModelSpeed.w = 0;
            mowerMotorControlDataForTest.cmdByRPM.keepAliveEnable = 0;
            mowerMotorControlDataForTest.cmdType = MOTOR_CONTROL_BY_MODEL;
        }
        LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Rev motor run cmd pos:%d ctrl:%d\r\n", pos, ctrl);
    }
}

static void getMotorStatus(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    int16_t temp_data = 0;
    uint8_t pos = *pu8Data;
    uint8_t index = *(pu8Data + 1);
    uint8_t type = *(pu8Data + 2);

    if(index > 1 || (pos > 1))
    {
        return;
    }

    if(COMM_CMD_CMAP_RD == u8Cmd)
    {
        if(type == 1)
        {
            temp_data = ABS_M(motorInterface.dev[pos].motorDriver[index]->status.Spd);
        }
        else if(type == 2)
        {
            temp_data = ABS_M(motorInterface.dev[pos].motorDriver[index]->status.Iq);
        }
        
        packData2Ap((uint8_t *)&temp_data, 2, COMM_SALED_READ_MOTOR_STATUS, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
    }
    LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Rev motor read pos:%d index:%d type:%d spd:%d\r\n", pos, index, type, temp_data);
}
#endif
// static void getRightMotorSpeed(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
// {
//     int16_t temp_data = 0;

//     if(COMM_CMD_CMAP_RD == u8Cmd)
//     {
//         temp_data = ABS_M(motorInterface.dev[0].motorDriver[1]->status.Spd);
//         packData2Ap((uint8_t *)&temp_data, 2, COMM_SALED_RIGHT_MOTOR_SPEED, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
//     }
//     LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Rev right motor read spd:%d\r\n", temp_data);
// }

// static void getLiftMotorIq(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
// {
//     int16_t temp_data = 0;

//     if(COMM_CMD_CMAP_RD == u8Cmd)
//     {
//         temp_data = ABS_M(motorInterface.dev[0].motorDriver[0]->status.Iq);
//         packData2Ap((uint8_t *)&temp_data, 2, COMM_SALED_LEFT_MOTOR_IQ, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
//     }
//     LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Rev left motor read iq:%d\r\n", temp_data);
// }

// static void getRightMotorIq(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
// {
//     int16_t temp_data = 0;

//     if(COMM_CMD_CMAP_RD == u8Cmd)
//     {
//         temp_data = ABS_M(motorInterface.dev[0].motorDriver[1]->status.Iq);
//         packData2Ap((uint8_t *)&temp_data, 2, COMM_SALED_RIGHT_MOTOR_IQ, COMM_CMD_ACK_RD, ID_SELF, u8SrcId);
//     }
//     LOG_TS(AM_LOG_LEVEL_FATAL,"cpu","Rev right motor read iq:%d\r\n", temp_data);
// }

#if 0
static void recAppStop(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if ((COMM_CMD_CAMP_WR == u8Cmd) || (COMM_CMD_CAMP_WR_NR == u8Cmd))
    {
        g_u16FacAppStop = *(uint16_t*)pu8Data;
        LOG_TS(AM_LOG_LEVEL_INFO, "FACTCK","recAppStop %d\n\r",g_u16FacAppStop);
    }
}
#endif

static CmdHandleList_st s_stApPdaCmdList[] = {
    {COMM_HOST_FACTORY_TEST_MODE_CONTROL,       switchFactoryMode},
#if defined (MODULE_INCLUDE_SCREEN)
    {COMM_PDA_SCREEN_CHECK,                     screenDisplayTest},
#endif
#if defined (MODULE_INCLUDE_SUBG)
    {COMM_PDA_CHARGE_STATION_PN,                chargeStationPn},
    {COMM_PDA_SUBG_PAIR_CMD,                    rcvPdaPaircmd},
    {COMM_PDA_SUB_SIGNAL_RX_STRENGTH,           readSubgRXSignal},
    {COMM_PDA_SUB_SIGNAL_TX_STRENGTH,           readSubgTXSignal},
    {COMM_PDA_ECU_SUBG_CFG,                     subgCfg},
    {COMM_SALED_SUBG_RX_RSSI,                   readSubgRxRSSI},
    {COMM_SALED_SUBG_TX_RSSI,                   readSubgTxRSSI},
#endif
#if defined (MODULE_INCLUDE_LED)
    {COMM_PDA_COLOR_CHECK,                      colorEffectTest},
#endif
#if defined (MODULE_INCLUDE_KEY)
    {COMM_PDA_STOP_KEY_CHECK,                   stopKeyCheck},
    {COMM_PDA_KEY_CHECK,                        funKeyCheck},
#endif
    
    {COMM_PDA_SHUT_DOWN,                        pdaShutDown},
    {COMM_PDA_ECU_FIRMWARE_VERSION,             getEcuVersion},
    {COMM_PDA_ECU_PN,                           getEcuPn},
    {COMM_PDA_ECU_RANDOM_NUM,                   getEcuRandomNum},
    {COMM_PDA_ECU_CPUID,                        getCpuId},
#if defined (MODULE_INCLUDE_MOTOR)
    {COMM_PDA_LAWN_MOTOR_START_STOP,            ctrlLawnMotor},
    {COMM_PDA_LAWN_MOTOR_SPEED_FEEDBACK,        getLawnMotorSpeed},
    {COMM_SALED_DRIVER_MOTOR_CMD,               ctrlRunMotor},
    {COMM_SALED_READ_MOTOR_STATUS,              getMotorStatus},
    {COMM_PDA_LIFT_MOTOR_HALL_TEST,             ctrlLiftMotor},
#endif
#if defined (MODULE_INCLUDE_BMS)
    {COMM_PDA_BAT_SOC,                          getBatSoc},
    {COMM_PDA_BAT_ERROR,                        getBatErr},
    {COMM_PDA_BAT_PARALLEL,                     getBatParallel},
    {COMM_PDA_BAT_NTC,                          getBatNTC},
    {COMM_PDA_BAT_MRFS,                         getBatMrfs},
    {COMM_PDA_BAT_PN,                           getBatPN},
    {COMM_PDA_BAT_INFOR,                        getBatInfor},
#endif
#if defined (MODULE_INCLUDE_BUMPER)
    {COMM_PDA_GET_BUMPER_STATUS,                getBumperStatus},
#endif
#ifdef TCU_MODULE
    {COMM_PDA_TCU_MOTOR_START_STOP,             ctrlTrimmerMotor},
    {COMM_PDA_TCU_MOTOR_SPEED_FEEDBACK,         getTrimmerMotorSpeed},
    {COMM_PDA_TCU_MOTOR_INSERT_STATUS,          tcuMotorInsertStatus},
#endif
    // {COMM_SALED_APP_STOP,                       recAppStop},
    // {COMM_SALED_RIGHT_MOTOR_SPEED,              getRightMotorSpeed},
    // {COMM_SALED_LEFT_MOTOR_IQ,                  getLiftMotorIq},
    // {COMM_SALED_RIGHT_MOTOR_IQ,                 getRightMotorIq},
#if defined (MODULE_INCLUDE_LIFTUP)
    {COMM_SALED_RIGHT_LIFT_HALL,                getHallValue},
#endif
    {COMM_PDA_ECU_BOOT_VERSION,                 getBootVersion},
};

void doApPdaCmd(uint8_t *pu8Data, uint8_t u8DataSize, uint8_t u8Index, uint8_t u8Cmd, uint8_t u8SrcId)
{
    if (u8Cmd < COMM_CMD_CMAP_RD || u8Cmd > COMM_CMD_ACK_WR)
    {
        LOG_TS(AM_LOG_LEVEL_FATAL, "FACTCK", "Inavlid cmd %d\r\n", u8Cmd);
        return;
    }

    if (COMM_CMD_CAMP_WR == u8Cmd)
    {
        packData2Ap(&u8Index, 1, 0, COMM_CMD_ACK_WR, ID_SELF, u8SrcId);   // all ack to AP is responsed here
    }

    for (uint32_t i = 0; i < sizeof(s_stApPdaCmdList)/sizeof(CmdHandleList_st); i++)
    {
        if ((u8Index == s_stApPdaCmdList[i].s32Index) && (NULL != s_stApPdaCmdList[i].pfnHandle))
        {
            s_stApPdaCmdList[i].pfnHandle(pu8Data, u8DataSize, u8Cmd, u8SrcId);
        #if defined (MODULE_INCLUDE_LED) || defined (MODULE_INCLUDE_SCREEN)
            if(s_stApPdaCmdList[i].s32Index != COMM_PDA_COLOR_CHECK && s_stApPdaCmdList[i].s32Index != COMM_PDA_SCREEN_CHECK)
            {
                clr_display_test_status();                  //Prevent light effect and sreen check from failing to exit
            }
            break;
        #endif
        }
    }
}
